Modeling and System Identification using Extended Kalman Filter for a Quadrotor System

Quadrotor has emerged as a popular testbed for Unmanned Aerial Vehicle (UAV) research due to its simplicity in construction and maintenance, and its vertical take-off, landing and hovering capabilities. It is a flying rotorcraft that has four lift-generating propellers; two of the propellers rotate clockwise and the other two rotate counter-clockwise. This paper presents modeling and system identification for autostabilization of a quadrotor system through the implementation of Extended Kalman Filter (EKF). EKF has known to be typical estimation technique used to estimate the state vectors and parameters of nonlinear dynamical systems. In this paper, two main processes are highlighted; dynamic modeling of the quadrotor and the implementation of EKF algorithms. The aim is to obtain a more accurate dynamic model by identify and estimate the needed parameters for the quadrotor. The obtained results demonstrate the performances of EKF based on the flight test applied to the quadrotor system. Keywords: Extended Kalman Filter (EKF), system identification, quadrotor system. 1. Introduction In general, Kalman Filter (KF) was first developed in 1960, and since then it has been a topic of extensive research with many applications. A wide range of variations of the original filter have been developed and some of the most common applications are in the area of autonomous or assisted navigation, object tracking or even econometric applications [1][3]. The application of KF to nonlinear systems can be difficult. Thus, the work was extended to the most common approach, the extended Kalman Filter (EKF), which linearizes the nonlinear model so that the traditional linear KF can be applied [2][4]. In this research, the testbed used is a four-rotor Unmanned Air Vehicle (UAV), known as quadrotor. A quadrotor is referred to as a small agile vehicle, which has four rotors located at the front, rear, left, and right ends of a cross frame. It requires no cyclic or collective pitch. Quadrotor can be highly maneuverable, and has the potential to hover, take off, fly and land in small areas. It is mechanically simple and is controlled by only changing the speed of rotation of the four rotors. Fig. 1: Quadrotor Flying-robot As illustrated in Fig. 1, the quadrotor used as the testbed of this study is a modified commercially available Remote Controlled (R/C) quadrotor. However, its electronics were replaced with our own developed on-board components. It consists of a flight stabilizer sensor, which is a six degree of freedom Inertial Measurement Unit (IMU), an ultrasonic sensor, a transducer and a microcontroller. The IMU consists of three analog gyros and three analog accelerometers, and is used to directly measure the Euler angles and the angular rates for x, y, z axes, respectively. The test data obtained from the running flight test are then used in the EKF algorithm to provide accurate aerodynamic parameters. ISBN 978-1-84626-xxx-x Proceedings of 2012 International Conference on Electrical Engineering and Applications (ICEEA 2012) Bali, Indonesia, 15-16 December, 2012, pp. xxx-xxx The procedure started by running the model with a set of flight modes; take off, hover and landing in an indoor structured environment. Then, data was acquired initially from trim conditions, and ended with treating the output data using EKF using MATLAB. This paper is organized as follows. Section II presents quadrotor dynamic modeling. Section III discusses the system identification techniques, the implementation of EKF and applied to the quadrotor. This section also analyzes the results obtained, in form of values and graphs. Section IV concludes the paper. 2. Quadrotor Dynamic Modeling Two frames of reference have been used in the modeling the quadrotor. These are the earth inertial frame (denoted by subscript I) and the body-fixed frame (denoted by subscript B). The nonlinear dynamics of the vehicle are described by sets of equations of motion that are derived based on the Newton-Euler formalism. The free body diagram of the quadrotor is shown in Fig. 2 where each rotor produces a lift force and moment. Fig. 2: Quadrotor inertial and body frame. The assumption used for dynamic modeling as stated in [6][8] 1. The structure of quadrotor is rigid and symmetrical. 2. The centre of mass of quadrotor coincides with the origin of body frame, B. 3. The propellers are rigid in plane and the pitch is fixed. 2.1. Equations of motion The first set of differential equations of motion that describes the acceleration of the quadrotor can be written as: Λ = , , Λ = cφsθsψ-sφcψ cφsθsψ-sφcψ gcφcθ (1) where c and s denote the cosine and sine, respectively, φ, θ and ψ are the roll, pitch and yaw angle in the body frame, respectively, g is the gravitational acceleration (9.8 m/s 2 ), m is the total mass of the quadrotor; x, y and z are the x, y, z position of the vehicle and u1 is the full throttle. While, the second set of the equations of motion is obtained by considering the angular momentum balance equation in body frame. Considering four kinematics movements for quadrotor which are full throttle movement (u1), roll movement (u2), pitch movement (u3), and yaw movement (u4) as the input variables in real application, the artificial input variables can be defined as follows: [7] = ! + !! + #! + $! (2) ! = #! − $! (3) # = ! − !! (4) $ = & ! + !! − #! − $! (5) where , !, #, $ are the angular speed for rotor 1, 2, 3 and 4, respectively, b is the thrust factor and d is the drag factor which are defined based on the results generated through force lift test done prior to flight test. Full throttle movement is provided by increasing or decreasing the velocity of all rotors by the same amount. For roll movement, along the x-axis of the body frame, the angular velocity of rotor (2) is increased and the angular velocity of rotor (4) is decreased while keeping the whole thrust constant. Similarly, the angular velocity of rotor (3) is increased and the angular velocity of rotor (1) is decreased to produce a pitch movement along y-axis of the body frame and for yawing movement, along z-axis of the body frame, the velocity of rotors (1,3) are increased and decreased the velocity of rotors (2,4). The second set of differential equation is as follows: [8] Λ ' = () , * , + , Λ = ./ // 0*1+1 234536 37 8 − 39 37 *1:; + < 37 ! )1+1 236537 34 8 − 39 34 )1:; + < 34 # *1)1 237534 36 8 + < 36 $ => >> ? (6) where :; is the gyroscopic effect. 2.2. Quadrotor parameter Table I illustrates quadrotor parameters calculated and measured from the testbed used for this paper. TABLE I. QUADROTOR PARAMETERS Parameters Value Unit Remark m 0.65 kg Mass of quadrotor L 0.165 m Length from motor to motor b 2.107 x 10 -5 kg.m 2 Lift (Thrust) factor d 4.566 x 10 -7 kg.m 2 Drag factor 3. Extended Kalman Filter (EKF) In general, EKF is referring to a Kalman filter that employs linearization at each time step to approximate the nonlinearities. EKF approximates the current mean and covariance using the first order approximation of the system dynamics. The EKF time update (prediction stage) is described as follows [1][2][5] For the estimated state: XAB= f X AB, UB, 0 (7) For the error covariance: PB5 = ABPB5 AB H +WBQB5 WBH (8) where A, and W is Jacobian matrix of partial derivatives of function f with respect to the x and w, respectively. K L,M = NO L NPQ M RST5 , UT5 , 0 (9) V L,M = NO L NW M RST5 , UT5 , 0 (10) While the EKF measurement update (correction stage) is described as follows. For the Kalman Gain, Kk: XT = YT5ZT (ZTYT5ZT + [T\T[T ,5 (11) where H, and V are the Jacobian matrix of partial derivatives of function h with respect to the x and v, respectively. Z L,M = N] L NP M R̂, 0 , [ L,M = N] L N_ M R̂, 0 (12) For the estimation update with measurement zk: R̀a = R̀a− + Xa a−h R̀a−, 0 (13) For the error covariance update, Pk: YT = c − XTZT5 YT5 (14) Basically, identification of parameters through filtering approach transformed the parameter estimation problem into a state estimation problem. It employs a continuous estimation model for prediction and the measurements that are recorded at discrete time step for the correction. The system dynamic of the quadrotor is represented in the continuous state space equation along with the measurement model as follows [8]. R1 = KR + dU + eV (15) f = gR +h[ (16) Since the state vector of the quadrotor: X = (x, y, z, x1 , y1 , z1 , φ, θ, ψ, φ1 , θ1 , ψ1 ,H (17) Thus, the quadrotor system equations with state and input matrices are given by equations (18-19). For the measurement matrix, it involved the same variables as in state space equations, which are: R1 = R$, Ro, Rp, Λ , R q, R , R !, Λ ' (18) Equation (19) can be rewritten in a form of equation (15) and (16) with L and M equal to zero. R1 = 0#r# 0#r# 0#r# 0#r# 1#r# e 0#r# 0#r# 0#r# 0#r# 0#r# 0#r# 0#r# 0#r# 1#r# e! R + 0#r e# 0#r 0#r 0#r# 0#r# 0#r# e$ t ! # $u (19) where: e = v0 0 0 0 0 0 0 0 :w , e! = ./ /0 0 +1 234536 37 8 − 39 37 :; 0 +1 236537 34 8 − 39 34 :; 0 0 )1 237534 36 8 0 0=> >> ? , e# = .// /0 xyz{x|}zyz| ~ 5 xyz{z|5zyx| ~ 5 xyx{ ~ => >>? and e$ =  < 37 < 3€ 36 Therefore, the measurement matrix is derived as: f~‚ƒ„ = &…†:‡ˆ†‰ Š†‹Œ… , , , 1 , 1 , 1 , ∅, *, +, ∅1 , *1 , +1 In order to run the Kalman filter, a set of data is obtained; these measurements are compared with the filter state predictions. Then, the differences are used by EKF to converge and correct the results. For the measurement model, the responds of quadrotor for a sequence of flight missions, starting from low speed motor rotation to high speed motor rotation, followed by take-off, hover and landing are analyzed. The time taken for a full flight test is 90 seconds. The sampling rate for both algorithms is 10 Hz which gives around 900 iterations. In the implementation of EKF, the filter has been tuned with adequate measurement (R matrix) and process (Q matrix) noise covariance. Fig. 3(a): EKF state filtering; measured and estimated for velocity at x, y, and z-axes respectively. Fig. 3(b): Error computed for velocity at x, y, and z-axes respectively. Fig. 4(a): EKF state filtering; measured and estimated for angular velocity at x, y, and z-axes respectively. Fig. 4(b): Error computed for angular velocity at x, y, and z-axes. 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 -2 -1 0 1